import copy
import math
import numpy as np
import matplotlib.pyplot as plt
from queue import PriorityQueue
from numpy.core.fromnumeric import shape
from enum import Enum
import time
from Source.CWJ.gdzbzh import pzjc


# graph为矩阵
def a_star_search(graph, start, goal):
    frontier = PriorityQueue()
    frontier.put((0, start))
    came_from = dict()
    cost_so_far = dict()
    turn = dict()
    came_from[start] = None
    cost_so_far[start] = 0
    # print(goal)
    while not frontier.empty():
        current = frontier.get()
        if current == goal:
            break

        for next in neighbors(graph, current[1]):
            new_cost = cost_so_far[current[1]] + 1

            if current[1] != start and isturn(current[1], next, turn[current[1]]) == 1:
                new_cost = new_cost + 20

            if next not in cost_so_far or new_cost < cost_so_far[next]:
                if next[0] - current[1][0] == 1:
                    turn[next] = 0
                if next[0] - current[1][0] == -1:
                    turn[next] = 1
                if next[1] - current[1][1] == 1:
                    turn[next] = 2
                if next[1] - current[1][1] == -1:
                    turn[next] = 3
                if next[2] - current[1][2] == 1:
                    turn[next] = 4
                if next[2] - current[1][2] == -1:
                    turn[next] = 5

                cost_so_far[next] = new_cost
                priority = new_cost + heuristic(goal, next)
                frontier.put((priority, next))
                came_from[next] = current[1]
    current_my = copy.deepcopy(goal)
    my_list = []
    while (current_my != start):
        my_list.append(current_my)
        current_my = came_from[current_my]
    my_list.append(start)
    my_list.reverse()
    return my_list


# 寻找临近点
def neighbors(graph, current1):
    coordinate = []
    current = copy.deepcopy(current1)
    if (current[0] != 0 and (graph[current[0] - 1, current[1], current[2]] == 255 or graph[
        current[0] - 1, current[1], current[2]] == 0)):
        coordinate.append((current[0] - 1, current[1], current[2]))
    if (current[0] != graph.shape[0] - 1 and (graph[current[0] + 1, current[1], current[2]] == 255 or graph[
        current[0] + 1, current[1], current[2]] == 0)):
        coordinate.append((current[0] + 1, current[1], current[2]))
    if (current[1] != 0 and (graph[current[0], current[1] - 1, current[2]] == 255 or graph[
        current[0], current[1] - 1, current[2]] == 0)):
        coordinate.append((current[0], current[1] - 1, current[2]))
    if (current[1] != graph.shape[1] - 1 and (graph[current[0], current[1] + 1, current[2]] == 255 or graph[
        current[0], current[1] + 1, current[2]] == 0)):
        coordinate.append((current[0], current[1] + 1, current[2]))
    if (current[2] != 0 and (graph[current[0], current[1], current[2] - 1] == 255 or graph[
        current[0], current[1], current[2] - 1] == 0)):
        coordinate.append((current[0], current[1], current[2] - 1))
    if (current[2] != graph.shape[2] - 1 and (graph[current[0], current[1], current[2] + 1] == 255 or graph[
        current[0], current[1], current[2] + 1] == 0)):
        coordinate.append((current[0], current[1], current[2] + 1))
    return coordinate


# 是否转弯
def isturn(current1, next1, direction1):
    current = copy.deepcopy(current1)
    next = copy.deepcopy(next1)
    direction = copy.deepcopy(direction1)
    if (next[0] - current[0]) == 1 and direction == 0:
        return 0
    elif (next[0] - current[0]) == -1 and direction == 1:
        return 0
    elif (next[1] - current[1]) == 1 and direction == 2:
        return 0
    elif (next[1] - current[1]) == -1 and direction == 3:
        return 0
    elif (next[2] - current[2]) == 1 and direction == 4:
        return 0
    elif (next[2] - current[2]) == -1 and direction == 5:
        return 0
    return 1


# 两点距离
def heuristic(a, b):
    a1 = copy.deepcopy(a)
    b1 = copy.deepcopy(b)
    return (abs(a1[0] - b1[0]) + abs(a1[1] - b1[1]) + abs(a1[2] - b1[2])) * 1


def zhuanhuan(point1, point2):
    a = shape(point1)[0]
    b = np.zeros((8, 3))
    x1 = point1[0]
    y1 = point1[1]
    z1 = point1[2]
    x2 = point2[0]
    y2 = point2[1]
    z2 = point2[2]
    b = [[x1, y1, z1], [x1, y1, z2], [x2, y1, z1], [x2, y1, z2], [x1, y2, z1], [x1, y2, z2], [x2, y2, z1], [x2, y2, z2]]
    return (b)


# 输入障碍物底面xy坐标，得到障碍物底面包含的所有点
def point_area(point_1, high):
    for i in range(4):
        if point_1[i][0] == min(point_1[:, 0]):
            point1 = point_1[i]
        if point_1[i][0] == max(point_1[:, 0]):
            point4 = point_1[i]
        if point_1[i][1] == min(point_1[:, 1]):
            point2 = point_1[i]
        if point_1[i][1] == max(point_1[:, 1]):
            point3 = point_1[i]
    z = point1[2]
    x1 = point1[0]
    y1 = point1[1]
    x2 = point2[0]
    y2 = point2[1]
    x3 = point3[0]
    y3 = point3[1]
    x4 = point4[0]
    y4 = point4[1]
    k1 = (x1 - x2) / (y1 - y2)
    k2 = (x1 - x3) / (y1 - y3)
    k3 = (x4 - x2) / (y4 - y2)
    k4 = (x3 - x4) / (y3 - y4)
    long_y = max(y1, y2, y3, y4) - min(y1, y2, y3, y4)
    long_x = max(x1, x2, x3, x4) - min(x1, x2, x3, x4)
    point_start = [[int(x2), int(y2), z, 2]]
    point_over = [[int(x2) + 1, int(y2) + 1, z + high]]
    for i in range(int(long_y)):
        y = int(y2) + i + 1
        if y <= y1:
            x_1 = x2 + (1 / k1) * (y - y2)
        if y > y1:
            x_1 = x1 + (y - y1) * (1 / k2)
        if y <= y4:
            x_2 = x2 + (y - y2) * (1 / k3)
        if y > y4:
            x_2 = x4 + (y - y4) * (1 / k4)
        point_start.append([int(x_1), y, z, 2])
        point_over.append([math.ceil(x_2) + 1, y + 1, z + high])
    return point_start, point_over


# 新的建立模型函数，point是障碍物xy平面所有点，high是障碍物起始高度和终止高度
def convert_to_matrix_new(point, diameter, goal1, high, area):
    a = int(area[0] / diameter) + 1
    b = int(area[1] / diameter) + 1
    c = int(area[2] / diameter) + 1
    zhangai = np.zeros((a, b, c))
    for u in range(0, shape(point)[0]):
        if point[u][0] != 0:
            b = b + 1
    point_1 = np.zeros((b, 4))
    point_2 = np.zeros((b, 3))
    for u in range(b):
        for i in range(high[0], high[1] + 1):
            point_1[u][0] = point[u][0]
            point_1[u][1] = point[u][1]
            point_1[u][2] = i
            point_1[u][3] = 2
            point_2[u][0] = point[u][0] + 1
            point_2[u][1] = point[u][1] + 1
            point_2[u][2] = i + 1
            zhangai[[point[u][0]], [point[u][1]], [i]] = 2
    middle_point_1 = np.zeros((shape(goal1)[0] - 1, 4))
    middle_point_2 = np.zeros((shape(goal1)[0] - 1, 3))
    for u in range(shape(goal1)[0] - 1):
        middle_point_1[u][0] = goal1[u][0]
        middle_point_1[u][1] = goal1[u][1]
        middle_point_1[u][2] = goal1[u][2]
        middle_point_2[u][0] = goal1[u][0] + 1
        middle_point_2[u][1] = goal1[u][1] + 1
        middle_point_2[u][2] = goal1[u][2] + 1
        middle_point_1[u][3] = 0
        zhangai[[goal1[u][0]], [goal1[u][1]], [goal1[u][2]]] = 0
        # plt.axis("off")#这一行是去掉所有的坐标线和坐标轴
    # plt.show()
    np.save('zhangai.npy', zhangai)
    return zhangai, point_1, point_2, middle_point_1, middle_point_2


# 这个就是建立模型的函数
def convert_to_matrix(point1, point2, diameter, goal1, area):
    # 建立图纸xy平面100*100
    a = int(area[0] / diameter) + 5
    b = int(area[1] / diameter) + 5
    c = int(area[2] / diameter) + 5
    a = 2*a
    b = 2*b
    c = 2*c
    zhangai = np.zeros((a, b, c))
    # point存储的值分别为障碍物的x,y,z坐标以及障碍物的类型即1型为可穿透障碍物，2型为不可穿透障碍物
    # point1 = [[1000, 1000, 0, 1], [4000, 3000, 3000, 2], [8000, 6000, 0, 2], [0, 6000, 0, 1],[2000,2000,2000,2],[6000,1000,500,1],[4000,0,0,2],[8500,5000,5500,1]]
    # point2 = [[2000, 4000, 2000], [6000, 5000, 7000], [9700, 4000, 5000], [2500, 9900, 9900],[3000,3000,3000],[7000,2000,1500],[5000,2000,3000],[9500,6000,7000]]
    b = 0
    for u in range(0, shape(point1)[0]):
        if point1[u][0] != 0 or point2[u][0] != 0:
            b = b + 1
    point_1 = np.zeros((b, 4))
    point_2 = np.zeros((b, 3))
    for u in range(b):
        # 通过输入点的坐标来确定面的偏移值,所有坐标均按照输入管道直径等比例缩小存储到矩阵
        x1 = int(min(point1[u][0], point2[u][0]) / diameter)
        x2 = math.ceil(max(point1[u][0], point2[u][0]) / diameter)
        y1 = int(min(point1[u][1], point2[u][1]) / diameter)
        y2 = math.ceil(max(point1[u][1], point2[u][1]) / diameter)
        z1 = int(min(point1[u][2], point2[u][2]) / diameter)
        z2 = math.ceil(max(point1[u][2], point2[u][2]) / diameter)
        # 如果矩阵的边恰好整除diameter则在矩阵中的位置加一 例如障碍物右边坐标为2000，diame为100 则x2=21 因为2000属于2000到2199

        # 如果xyz超出矩阵范围则强制改为矩阵的边界
        if x1 < 0:
            x1 = 0
        if y1 < 0:
            y1 = 0
        if z1 < 0:
            z1 = 0
        if x2 > a or x2 == a:
            x2 = a - 1
        if y2 > b or y2 == b:
            y2 = b - 1
        if z2 > c or z2 == c:
            z2 = c - 1
        # print(x1,y1, z1,x2,y2, z2)#输出每个面相对与基础面的偏移值
        # 将障碍物存储到三维矩阵中，将对应位置的数置1
        point_1[u][0] = x1 + a/2
        point_1[u][1] = y1 + b/2
        point_1[u][2] = z1 + c/2
        point_2[u][0] = x2 + a/2
        point_2[u][1] = y2 + b/2
        point_2[u][2] = z2 + c/2
        point_1[u][3] = point1[u][3]
        for i in range(x1, x2 + 1):
            for j in range(y1, y2 + 1):
                for k in range(z1, z2 + 1):
                    zhangai[[i], [j], [k]] = point1[u][3]
    if shape(goal1)[0] > 1:
        middle_point_1 = np.zeros((shape(goal1)[0] - 1, 4))
        middle_point_2 = np.zeros((shape(goal1)[0] - 1, 3))
        for u in range(shape(goal1)[0] - 1):
            middle_point_1[u][0] = goal1[u][0]
            middle_point_1[u][1] = goal1[u][1]
            middle_point_1[u][2] = goal1[u][2]
            middle_point_2[u][0] = goal1[u][0] + 1
            middle_point_2[u][1] = goal1[u][1] + 1
            middle_point_2[u][2] = goal1[u][2] + 1
            middle_point_1[u][3] = 0
            zhangai[[goal1[u][0]], [goal1[u][1]], [goal1[u][2]]] = 0
    if shape(goal1)[0] <= 1:
        middle_point_1 = [0, 0, 0, 0]
        middle_point_2 = [0, 0, 0]
        # plt.axis("off")#这一行是去掉所有的坐标线和坐标轴
    # plt.show()
    np.save('zhangai.npy', zhangai)
    return zhangai, point_1, point_2, middle_point_1, middle_point_2


def install(point1, point2, condition):
    point_1 = np.zeros((shape(point1)[0], 4))
    point_2 = np.zeros((shape(point1)[0], 3))
    for u in range(shape(point1)[0]):
        for i in range(3):
            a = point1[u][i] - condition
            b = point2[u][i] + condition
            if a < 0:
                a = 0
            if a > 10000:
                a = 10000
            if b < 0:
                b = 0
            if b > 10000:
                b = 10000
            point_1[u][i] = a
            point_2[u][i] = b
        point_1[u][3] = point1[u][3]
    return point_1, point_2


def draw(StructWorkArea3D, current, res_list):
    height = current[4]
    width = current[5]
    if current[3] == 0:
        for i in range(current[1], current[1] + height):
            for j in range(current[2], current[2] + width):
                res_list.append([current[0], [i], [j]])
                StructWorkArea3D[current[0]][i][j] = 61

    if current[3] == 1:
        for i in range(current[0], current[0] + height):
            for j in range(current[2], current[2] + width):
                res_list.append([[i], current[1], [j]])
                StructWorkArea3D[i][current[1]][j] = 61

    if current[3] == 2:
        for i in range(current[0], current[0] + width):
            for j in range(current[1], current[1] + height):
                res_list.append([[i], [j], current[2]])
                StructWorkArea3D[i][j][current[2]] = 61


def inset(gragh, paths, g):
    for i in range(shape(paths)[0]):
        gragh[[paths[i][0]], [paths[i][1]], [paths[i][2]]] = g
    return gragh


def run1(point1, point2, start, goal, g, num, diameter, f, goal1, area):  # g为管道颜色，f为判断图形输出
    # point1 = [[1000, 1000, 0, 1], [4000, 3000, 3000, 2], [8000, 6000, 0, 2], [0, 6000, 0, 1], [2000, 2000, 2000, 2],
    #           [6000, 1000, 500, 1], [4000, 0, 0, 2], [8500, 5000, 5500, 1]]
    # point2 = [[2000, 4000, 2000], [6000, 5000, 7000], [9700, 4000, 5000], [2500, 9900, 9900], [3000, 3000, 3000],
    #           [7000, 2000, 1500], [5000, 2000, 3000], [9500, 6000, 7000]]
    # 我稍微修改了一下 a_turn_b的输出，除了输出矩阵方程外还输出障碍物，直接在用障碍物坐标输出

    shuchu = convert_to_matrix(point1, point2, diameter, goal1, area)
    graph = shuchu[0]
    point_1 = shuchu[1]
    point_2 = shuchu[2]
    middle_point_1 = shuchu[3]
    middle_point_2 = shuchu[4]
    # mylist = a_star_search1(graph, (5, 5, 5), (90, 50, 80), 2, 4, 0, 2, 4, 2)
    # mylist = a_star_search(graph, start, goal)
    mylist = []
    starttime = time.perf_counter()
    if len(goal1) != 0:
        print("有节点")
        for i in range(len(goal1)):
            print("start goal", start, goal1[i])
            while graph[start[0], start[1], start[2]] >= 1:
                print("起点在障碍物内")
                start = [start[0], start[1], start[2] - 1]
                print("起点下移一格")
            print("start goal", start, goal)
            while graph[goal[0], goal[1], goal[2]] >= 1:
                print("终点在障碍物内")
                goal = [goal[0], goal[1], goal[2] - 1]
                print("终点下移一格")
            print("start goal", start, goal)
            paths, cost, pathl = a_star_3D2(graph, heuristic, start, goal1[i])  # 从起点到下一节点的路径
            print("paths:", paths)
            graph = inset(graph, paths, g)
            # if i>0:
            #     paths= np.delete(paths,[0],0)
            # print(paths)
            mylist = mylist + paths  # 路径点全部放在mylist中
            start = paths[-1]  # 起点等于上一路径的终点
            endtime = time.perf_counter()
            # print("endtime:", endtime)
            print("运行时间：", endtime - starttime)
            if endtime - starttime > 30:
                print("——————————超时")
                return False
                exit(0)

        paths, cost, pathl = a_star_3D(graph, heuristic, start, goal)
        graph = inset(graph, paths, g)
        mylist = mylist + paths
        print("mylist:", mylist)
    else:
        print("无节点")
        while graph[start[0], start[1], start[2]] >= 1:
            print("起点在障碍物内")
            start = [start[0], start[1], start[2] + 1]
            print("起点下移一格")
        print("start goal", start, goal)
        while graph[goal[0], goal[1], goal[2]] >= 1:
            print("终点在障碍物内")
            goal = [goal[0], goal[1], goal[2] + 1]
            print("终点下移一格")
        mylist, cost, pathl = a_star_3D(graph, heuristic, start, goal)
        endtime = time.perf_counter()
        # print("endtime:", endtime)
        print("运行时间：", endtime - starttime)
        if endtime - starttime > 30:
            print("——————————超时")
            return False
            exit(0)
        # print("mylist:", mylist)
    # 展示graph矩阵,其中管道进过的方格坐标在mylist中
    if not mylist:
        return mylist
    # print(mylist)
    color = g
    a = shape(mylist)
    b = 0
    v = np.zeros(a[0])
    w_1 = np.zeros((30, 3))
    w_2 = np.zeros((30, 3))
    w__1 = np.zeros((shape(mylist)[0], 3))
    w__2 = np.zeros((shape(mylist)[0], 3))
    # 赋值起点
    w_1[0] = mylist[0]
    # 赋值中间点
    for u in range(1, a[0]):
        if mylist[u][0] != mylist[u - 1][0]:
            v[u] = 0
        if mylist[u][1] != mylist[u - 1][1]:
            v[u] = 1
        if mylist[u][2] != mylist[u - 1][2]:
            v[u] = 2
    for u in range(1, a[0] - 1):
        if v[u] != v[u + 1]:
            w_2[b] = mylist[u]
            w_1[b + 1] = mylist[u + 1]
            b = b + 1
    # 赋值终点
    w_2[b] = mylist[a[0] - 1]
    # print(w_1,w_2)
    longer = longer1(w_1, w_2)

    fig = plt.figure("dandian", dpi=100)
    sub = fig.add_subplot(111, projection='3d')
    sub.set_xlabel(r'$x$')
    sub.set_ylabel(r'$y$')
    sub.set_zlabel(r'$z$')
    ch_mash = np.zeros((5, 5))
    x0 = np.linspace(0, 100, 5)
    y0 = x0
    x0_mash, y0_mash = np.meshgrid(x0, y0)
    for u in range(shape(mylist)[0]):
        g = color
        x1 = int(mylist[u][0])
        x2 = x1 + 1
        y1 = int(mylist[u][1])
        y2 = y1 + 1
        z1 = int(mylist[u][2])
        z2 = z1 + 1
        w__1[u] = [x1, y1, z1]
        w__2[u] = [x2, y2, z2]
        x = np.linspace(x1, x2, 5)
        y = np.linspace(y1, y2, 5)
        z = np.linspace(z1, z2, 5)
        x_z_mash, y_z_mash = np.meshgrid(x, y)
        y_x_mash, z_x_mash = np.meshgrid(y, z)
        x_y_mash, z_y_mash = np.meshgrid(x, z)
        z1_mash = ch_mash + z1
        z2_mash = ch_mash + z2
        x1_mash = ch_mash + x1
        x2_mash = ch_mash + x2
        y1_mash = ch_mash + y1
        y2_mash = ch_mash + y2
        if f == 1:
            sub.plot_surface(x0_mash, y0_mash, ch_mash, color='gray', alpha=0)
            sub.plot_surface(x0_mash, ch_mash, y0_mash / 2, color='gray', alpha=0)
        if u == 0:
            g = color + 2
        if u == shape(mylist)[0] - 1:
            g = color + 1
        sub.plot_surface(x_z_mash, y_z_mash, z1_mash, color=color_pip(g))
        sub.plot_surface(x_z_mash, y_z_mash, z2_mash, color=color_pip(g))
        sub.plot_surface(x1_mash, y_x_mash, z_x_mash, color=color_pip(g))
        sub.plot_surface(x2_mash, y_x_mash, z_x_mash, color=color_pip(g))
        sub.plot_surface(x_y_mash, y1_mash, z_y_mash, color=color_pip(g))
        sub.plot_surface(x_y_mash, y2_mash, z_y_mash, color=color_pip(g))
    for u in range(shape(point_1)[0]):
        # 通过输入点的坐标来确定面的偏移值,所有坐标均按比例缩小100倍
        x1 = point_1[u][0]
        x2 = point_2[u][0]
        y1 = point_1[u][1]
        y2 = point_2[u][1]
        z1 = point_1[u][2]
        z2 = point_2[u][2]
        x = np.linspace(x1, x2, 5)
        y = np.linspace(y1, y2, 5)
        z = np.linspace(z1, z2, 5)
        x_z_mash, y_z_mash = np.meshgrid(x, y)
        y_x_mash, z_x_mash = np.meshgrid(y, z)
        x_y_mash, z_y_mash = np.meshgrid(x, z)
        z1_mash = ch_mash + z1
        z2_mash = ch_mash + z2
        x1_mash = ch_mash + x1
        x2_mash = ch_mash + x2
        y1_mash = ch_mash + y1
        y2_mash = ch_mash + y2
        # 输出障碍物，绿色为可穿透障碍物，蓝色为不可穿透障碍物
        if f == 1:
            if u < shape(point1)[0]:
                sub.plot_surface(x_z_mash, y_z_mash, z1_mash, color=color_pip(int(point_1[u][3])), alpha=1)
                sub.plot_surface(x_z_mash, y_z_mash, z2_mash, color=color_pip(int(point_1[u][3])), alpha=1)
                sub.plot_surface(x1_mash, y_x_mash, z_x_mash, color=color_pip(int(point_1[u][3])), alpha=1)
                sub.plot_surface(x2_mash, y_x_mash, z_x_mash, color=color_pip(int(point_1[u][3])), alpha=1)
                sub.plot_surface(x_y_mash, y1_mash, z_y_mash, color=color_pip(int(point_1[u][3])), alpha=1)
                sub.plot_surface(x_y_mash, y2_mash, z_y_mash, color=color_pip(int(point_1[u][3])), alpha=1)
            else:
                sub.plot_surface(x_z_mash, y_z_mash, z1_mash, color=color_pip(int(point_1[u][3])))
                sub.plot_surface(x_z_mash, y_z_mash, z2_mash, color=color_pip(int(point_1[u][3])))
                sub.plot_surface(x1_mash, y_x_mash, z_x_mash, color=color_pip(int(point_1[u][3])))
                sub.plot_surface(x2_mash, y_x_mash, z_x_mash, color=color_pip(int(point_1[u][3])))
                sub.plot_surface(x_y_mash, y1_mash, z_y_mash, color=color_pip(int(point_1[u][3])))
                sub.plot_surface(x_y_mash, y2_mash, z_y_mash, color=color_pip(int(point_1[u][3])))
    if shape(goal1)[0] > 1:
        for u in range(shape(middle_point_1)[0]):
            # 通过输入点的坐标来确定面的偏移值,所有坐标均按比例缩小100倍
            x1 = middle_point_1[u][0]
            x2 = middle_point_2[u][0]
            y1 = middle_point_1[u][1]
            y2 = middle_point_2[u][1]
            z1 = middle_point_1[u][2]
            z2 = middle_point_2[u][2]
            x = np.linspace(x1, x2, 5)
            y = np.linspace(y1, y2, 5)
            z = np.linspace(z1, z2, 5)
            x_z_mash, y_z_mash = np.meshgrid(x, y)
            y_x_mash, z_x_mash = np.meshgrid(y, z)
            x_y_mash, z_y_mash = np.meshgrid(x, z)
            z1_mash = ch_mash + z1
            z2_mash = ch_mash + z2
            x1_mash = ch_mash + x1
            x2_mash = ch_mash + x2
            y1_mash = ch_mash + y1
            y2_mash = ch_mash + y2
            # 输出障碍物，绿色为可穿透障碍物，蓝色为不可穿透障碍物
            if f == 1:
                sub.plot_surface(x_z_mash, y_z_mash, z1_mash, color=color_pip(int(middle_point_1[u][3])))
                sub.plot_surface(x_z_mash, y_z_mash, z2_mash, color=color_pip(int(middle_point_1[u][3])))
                sub.plot_surface(x1_mash, y_x_mash, z_x_mash, color=color_pip(int(middle_point_1[u][3])))
                sub.plot_surface(x2_mash, y_x_mash, z_x_mash, color=color_pip(int(middle_point_1[u][3])))
                sub.plot_surface(x_y_mash, y1_mash, z_y_mash, color=color_pip(int(middle_point_1[u][3])))
                sub.plot_surface(x_y_mash, y2_mash, z_y_mash, color=color_pip(int(middle_point_1[u][3])))
    plt.savefig(num)
    # plt.axis('off')
    plt.savefig(num)
    if f==1:
        plt.show()
    #     plt.close()

    return w__1, w__2, g, mylist, longer, w_1, w_2


# 每一条管道运行完后将其输入为障碍物
def pipeline_to_barrier(point1, point2, w_1, w_2, color, diameter):  # 输入障碍物和上一节管道即可得到新的障碍物
    c = shape(point1)[0]
    d = shape(w_1)[0]
    e = 0
    for i in range(d):
        if w_2[i][0] != 0 or w_1[i][0] != 0:
            e = e + 1
    a = np.zeros(((c + e), 4))
    b = np.zeros(((c + e), 3))
    for i in range(c):
        for j in range(3):
            a[i][j] = point1[i][j]
            b[i][j] = point2[i][j]
        a[i][3] = point1[i][3]
    for i in range(c, c + e):
        for j in range(3):
            a[i][j] = int(w_1[i - c][j]) * diameter
            b[i][j] = int(w_2[i - c][j]) * diameter

        a[i][3] = color
    return a, b


def pipeline_to_barrier1(point1, point2, w_1, w_2, color, diameter, condition):
    a = np.zeros((4))
    c = shape(w_1)[0]
    d = point1
    f = point2
    w_1 = w_1 * diameter - condition
    w_2 = w_2 * diameter + condition - 1
    for i in range(c):
        for j in range(3):
            a[j] = w_1[i][j]
        if i == 0:
            a[3] = color + 2
        elif i == shape(w_1)[0] - 1:
            a[3] = color + 1
        else:
            a[3] = color
        b = w_2[i]
        d = np.row_stack((d, a))
        f = np.row_stack((f, b))
    return d, f


# 控制颜色
def color_pip(a):
    b = ['orange', 'gray', 'gray', 'blue', 'red', 'yellow', 'pink', 'olive', 'green', 'purple']
    return b[a]


# 输出管道长度
def longer1(w_1, w_2):
    c = 0
    a = 0
    for i in range(shape(w_1)[0]):
        if w_1[i][0] != 0 or w_1[i][1] != 0 or w_1[i][2] != 0 or w_2[i][0] != 0 or w_2[i][1] != 0 or w_2[i][2] != 0:
            a = a + 1
        for j in range(3):
            if abs(w_2[i][j] - w_1[i][j]) > 0:
                c = c + abs(w_2[i][j] - w_1[i][j])
    c = c + a
    return c


# 输出管道为坐标模式
def pip_point_out(w_1, w_2, diameter, min_point, pip):
    a = list(w_1[0])
    b = list(w_2[0])
    c = 1
    out1 = []
    for i in range(1, shape(w_1)[0]):
        if w_1[i][0] == 0 and w_1[i][1] == 0 and w_1[i][2] == 0 and w_2[i][0] == 0 and w_2[i][1] == 0 and w_2[i][
            2] == 0:
            break
        a = a + list(w_1[i])
        b = b + list(w_2[i])
        c += 1
    out = list(np.zeros((c, 2, 3)))
    for i in range(c):
        for j in range(3):
            out[i][0][j] = a[3 * i + j] * diameter + min_point[j] + diameter / 2
            out[i][1][j] = b[3 * i + j] * diameter + min_point[j] + diameter / 2
        out[i] = out[i].tolist()
        out[i].append(pip[2])
        out[i].append(pip[3])
    # print(type(out))
    return out


# 将起点终点宽高转换为八点
def dian2(p1, p2, width, height):  # 第二种即两个中心点z不一样
    u = 0
    p3 = np.zeros(3)
    p4 = np.zeros(3)
    p5 = np.zeros(3)
    p6 = np.zeros(3)
    p_1 = np.zeros(3)
    p_2 = np.zeros(3)
    p_3 = np.zeros(3)
    p_4 = np.zeros(3)
    p_5 = np.zeros(3)
    p_6 = np.zeros(3)
    p_7 = np.zeros(3)
    p_8 = np.zeros(3)
    if p1[1] - p2[1] == 0:
        u = 1
    if u == 0:
        k = -(p1[0] - p2[0]) / (p1[1] - p2[1])
        x = width / (2 * (1 + k ** 2) ** 0.5)
        p3[0] = p1[0] + x
        p3[1] = p1[1] + k * x
        p3[2] = p1[2]
        p4[0] = p1[0] - x
        p4[1] = p1[1] - k * x
        p4[2] = p1[2]
        p5[0] = p2[0] + x
        p5[1] = p2[1] + k * x
        p5[2] = p2[2]
        p6[0] = p2[0] - x
        p6[1] = p2[1] - k * x
        p6[2] = p2[2]
    else:
        p3[0] = p1[0]
        p3[1] = p1[1] + width / 2
        p3[2] = p1[2]
        p4[0] = p1[0]
        p4[1] = p1[1] - width / 2
        p4[2] = p1[2]
        p5[0] = p2[0]
        p5[1] = p2[1] + width / 2
        p5[2] = p2[2]
        p6[0] = p2[0]
        p6[1] = p2[1] - width / 2
        p6[2] = p2[2]
    a = (p2[1] - p1[1]) * (p3[2] - p1[2]) - (p3[1] - p1[1]) * (p2[2] - p1[2])
    b = (p2[2] - p1[2]) * (p3[0] - p1[0]) - (p3[2] - p1[2]) * (p2[0] - p1[0])
    c = (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p3[0] - p1[0]) * (p2[1] - p1[1])
    t = height / (2 * (a ** 2 + b ** 2 + c ** 2) ** 0.5)
    p_3[0] = p3[0] + a * t
    p_3[1] = p3[1] + b * t
    p_3[2] = p3[2] + c * t
    p_2[0] = p3[0] - a * t
    p_2[1] = p3[1] - b * t
    p_2[2] = p3[2] - c * t
    p_4[0] = p4[0] + a * t
    p_4[1] = p4[1] + b * t
    p_4[2] = p4[2] + c * t
    p_1[0] = p4[0] - a * t
    p_1[1] = p4[1] - b * t
    p_1[2] = p4[2] - c * t
    p_7[0] = p5[0] + a * t
    p_7[1] = p5[1] + b * t
    p_7[2] = p5[2] + c * t
    p_6[0] = p5[0] - a * t
    p_6[1] = p5[1] - b * t
    p_6[2] = p5[2] - c * t
    p_8[0] = p6[0] + a * t
    p_8[1] = p6[1] + b * t
    p_8[2] = p6[2] + c * t
    p_5[0] = p6[0] - a * t
    p_5[1] = p6[1] - b * t
    p_5[2] = p6[2] - c * t
    list = [p_3, p_7, p_6, p_2, p_4, p_8, p_5, p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new


def run(point1, point2, start, goal, color, num, diameter, goal1, condition, min_point, pip, area):
    a = shape(start)[0]
    f = 0
    if condition[0][1] > 0:
        install_1 = install(point1, point2, condition[0][1])
    if condition[0][1] == 0:
        install_1 = (point1, point2)
    longer = np.zeros(a)
    if a == 1:
        f = 1
    print("运行到第", 1, "节管道")
    point1 = install_1[0]
    point2 = install_1[1]
    diameter[0] = diameter[0] + 2 * condition[0][0]
    b = run1(point1, point2, tuple(start[0]), tuple(goal[0]), color[0], num, diameter[0], f, goal1[0], area)
    if b:
        pip_point_0 = pip_point_out(b[5], b[6], diameter[0], min_point, pip[0])  # 输出函数
        pip_last_out = pip_point_0
        longer[0] = b[4] * diameter[0]
        c = pipeline_to_barrier1(point1, point2, b[0], b[1], color[0], diameter[0], condition[0][1])
        w_1 = c[0]
        w_2 = c[1]
        for u in range(1, a):
            print("运行到第", u + 1, "节管道")
            if u == a - 1:
                f = 1
            diameter[u] = diameter[u] + 2 * condition[u][0]
            b = run1(w_1, w_2, tuple(start[u]), tuple(goal[u]), color[u], num, diameter[u], f, goal1[u], area)
            if b:
                pip_point = pip_point_out(b[5], b[6], diameter[u], min_point, pip[u])
                if u == 1:
                    pip_last_out = pip_point_0 + pip_point
                if u > 1:
                    pip_last_out = pip_last_out + pip_point
                longer[u] = b[4] * diameter[u]
                c = pipeline_to_barrier1(w_1, w_2, b[0], b[1], color[u], diameter[u], condition[u][1])
                w_1 = c[0]
                w_2 = c[1]
            else:
                print("第", u + 1, "节管道错误")
                return pip
    else:
        print("第", 1, "节管道错误")
        return pip
    return pip_last_out
    # for u in range(a):
    #     print('第',u+1,'条管道的长度为',longer[u])


def a_star_3D2(voxel, h, start, goal):
    path = []
    path_cost = 0
    queue = PriorityQueue()  # 优先级队列
    queue.put((0, start))  # 先进先出，数字越小优先级越高
    visited = set(start)  # start = (9,5,11), visited = {9,11,5} 去重且单独存放
    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()  # （0，start）
        current_node = item[1]  # item[1] = start
        if np.all(current_node == start):  # 做与运算，相等时输出True
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if h(current_node, goal) < 3:
            # if current_node in point_path:
            # print('Found a path.')
            found = True
            goal = current_node
            break
        else:
            for action in valid_actions_3D(voxel, current_node):
                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])
                # if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                #     cost_Nonconcrete = 100

                # branch_cost = current_cost + action.cost + (10 / (next_node[2]+1)) + cost_Nonconcrete  # z轴增加cost权重
                branch_cost = current_cost + action.cost
                queue_cost = branch_cost + h(next_node, goal)
                # queue_cost = branch_cost
                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))

                # for action in valid_actions_3D2(voxel, current_node):
                #     da = action.delta
                #     next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])
                #     # if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                #     #     cost_Nonconcrete = 100
                #
                #     # branch_cost = current_cost + action.cost + (10 / (next_node[2]+1)) + cost_Nonconcrete  # z轴增加cost权重
                #     branch_cost = current_cost + action.cost
                #     queue_cost = branch_cost + h(next_node, goal)
                # queue_cost = branch_cost
                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))

    if found:
        n = goal
        path_cost = branch[n][0]
        path.append(n)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        path_l = len(path[::-1])
    else:
        path_l = 500

    return path[::-1], path_cost, path_l


def a_star_3D(voxel, h, start, goal):
    path = []
    path_cost = 0
    queue = PriorityQueue()  # 优先级队列
    queue.put((0, start))  # 先进先出，数字越小优先级越高
    visited = set(start)  # start = (9,5,11), visited = {9,11,5} 去重且单独存放

    branch = {}
    found = False

    while not queue.empty():
        item = queue.get()  # （0，start）
        current_node = item[1]  # item[1] = start
        last_node = item[1]
        if np.all(current_node == start):  # 做与运算，相等时输出True
            current_cost = 0.0
        else:
            current_cost = branch[current_node][0]

        if np.all(current_node == goal):
            found = True
            break
        else:
            for action in valid_actions_3D(voxel, current_node):
                # get the tuple
                # cost_Nonconcrete = 0
                # bend_cost = 0

                da = action.delta
                next_node = (current_node[0] + da[0], current_node[1] + da[1], current_node[2] + da[2])
                # 加上非结构墙代价
                # if traj_2[next_node[0]][next_node[1]][next_node[2]] == True:
                #     cost_Nonconcrete = 250
                # 加上转弯代价
                # if (abs(last_node[0] - next_node[0]) == 1 and abs(
                #         last_node[1] - next_node[1]) == 1) or (
                #         abs(last_node[0] - next_node[0]) == 1 & abs(
                #     last_node[2] - next_node[2]) == 1) or (
                #         abs(last_node[2] - next_node[2]) == 1 & abs(
                #     last_node[1] - next_node[1]) == 1):
                #     bend_cost = 20

                branch_cost = current_cost + action.cost  # z轴增加cost权重
                queue_cost = branch_cost + h(next_node, goal)

                if next_node not in visited:
                    visited.add(next_node)
                    branch[next_node] = (branch_cost, current_node, action)
                    queue.put((queue_cost, next_node))
                    last_node = current_node

                """
                如果next_node在非结构墙中，cost+50？
                 （1）若穿非结构墙的cost很小，那么非结构墙相当于没有，可以自由穿过，聚一个类即可；
                 （2）若cost很大，就相当于结构墙，系统是不会让穿的，那么就会聚两个类。
                 那现在只需要判断next_node是否在非结构墙中，即某点是否在一个范围内
                """
    if found:
        n = goal
        path_cost = branch[n][0]
        path.append(goal)
        while branch[n][1] != start:
            path.append(branch[n][1])
            n = branch[n][1]
        path.append(branch[n][1])
        path_l = len(path[::-1])
    else:
        path_l = 500

    return path[::-1], path_cost, path_l


class Action3D(Enum):
    """
    An action is represented by a 4 element tuple.

    The first 3 values are the delta of the action relative
    to the current voxel position. The final value
    is the cost of performing the action.
    """

    WEST = (0, -1, 0, 1)
    EAST = (0, 1, 0, 1)
    NORTH = (-1, 0, 0, 1)
    SOUTH = (1, 0, 0, 1)

    # Up & down motions
    UP = (0, 0, 1, 1)
    DOWN = (0, 0, -1, 1)

    @property
    def cost(self):
        # print("cost_value", self.value)
        return self.value[3]

    @property
    def delta(self):
        # print("delta_value", self.value)
        return (self.value[0], self.value[1], self.value[2])


def valid_actions_3D(voxel, current_node):
    """
    Returns a list of valid actions given a voxel and current node.
    """
    valid_actions = list(Action3D)
    n, m, max_alt = voxel.shape[0] - 1, voxel.shape[1] - 1, voxel.shape[2] - 1
    x, y, z = current_node

    # check if the node is off the voxel or
    # it's an obstacle

    if z - 1 < 0 or voxel[x, y, z - 1] >= 1:
        valid_actions.remove(Action3D.DOWN)

    if z + 1 > max_alt or voxel[x, y, z + 1] >= 1:
        valid_actions.remove(Action3D.UP)

    if x - 1 < 0 or voxel[x - 1, y, z] >= 1:
        valid_actions.remove(Action3D.NORTH)

    if x + 1 > n or voxel[x + 1, y, z] >= 1:
        valid_actions.remove(Action3D.SOUTH)

    if y - 1 < 0 or voxel[x, y - 1, z] >= 1:
        valid_actions.remove(Action3D.WEST)

    if y + 1 > m or voxel[x, y + 1, z] >= 1:
        valid_actions.remove(Action3D.EAST)

    return valid_actions


def valid_actions_3D2(voxel, current_node):
    """
    Returns a list of valid actions given a voxel and current node.
    """
    valid_actions = list(Action3D)
    n, m, max_alt = voxel.shape[0] - 1, voxel.shape[1] - 1, voxel.shape[2] - 1
    x, y, z = current_node

    # check if the node is off the voxel or
    # it's an obstacle

    if voxel[x, y, z - 1] == 3:
        valid_actions.remove(Action3D.DOWN)
    if voxel[x, y, z + 1] == 3:
        valid_actions.remove(Action3D.UP)

    if voxel[x - 1, y, z] == 3:
        valid_actions.remove(Action3D.NORTH)

    if voxel[x + 1, y, z] == 3:
        valid_actions.remove(Action3D.SOUTH)

    if voxel[x, y - 1, z] == 3:
        valid_actions.remove(Action3D.WEST)
    if voxel[x, y + 1, z] == 3:
        valid_actions.remove(Action3D.EAST)

    return valid_actions


#
# point1 = [[0, 6000, 0, 1],[1000, 1000, 0, 1], [2000, 2000, 2000, 2],
# [4000, 3000, 3000, 2],[6000, 1000, 500, 1],  [8000, 6000, 0, 2], [8500, 5000, 5500, 1]]
# point2 = [ [2500, 9900, 9900], [2000, 4000, 2000], [3000, 3000, 3000],
# [6000, 5000, 7000],[7000, 2000, 1500], [9700, 4000, 5000],[9500, 6000, 7000]]
# start=[(3,9,2),(1,10,2)]
# goal=[(13,5,2),(15,6,2)]
# goal1 = [[],[]]  # goal1包括中间节点和终点
# color=[3,4]
# num="num1.png"
# diameter=[200,200]
# condition=[[0,0],[0,0]]
# run(point1,point2,start,goal,color,num,diameter,goal1,condition)


# pip1 = [[[204625.224403649, 171026.85355678, 55643.3], [209623.224403649, 171026.85355678, 55643.3], 250.0, 250.0],
#       [[209848.666575167, 171120.234761753, 55643.3], [211664.473679397, 172936.041865984, 55643.3], 250.0, 250.0]]
#
# block1 = [[[190499.494283648, 169090.192301657, 54900.0], [191299.494283648, 169090.192301657, 54900.0], 800.0, 4000.0],
#          [[191821.201035488, 169090.192301657, 54900.0], [192821.201035488, 169090.192301657, 54900.0], 1000.0, 4000.0],
#          [[189853.725801005, 167165.10170667, 55862.5], [194653.725801005, 167165.10170667, 55862.5], 1600.0, 75.0]]
#


# 将xml中读取的数据转换为运行a星算法的数据
def pip_block_to_point(block, pip, goal1, color, condition):
    print("block", block)
    if not block:
        print("没有碰撞，直接输出管道：", pip)
        # print("类型",type(pip))
        return pip

    block_num = 0
    tem_0 = pzjc(pip, block)
    for i in range(len(tem_0)):
        if tem_0[i]:
            block_num += 1

    a0 = shape(block)[0]
    point1 = []
    point2 = []
    point_1 = np.zeros((4, 3))
    for i in range(shape(block)[0]):
        b = np.zeros((8, 1))
        c = np.zeros((3, 1))
        c1 = np.zeros((3, 1))
        a = dian2(block[i][0], block[i][1], block[i][2], block[i][3])
        a = np.array(a)
        if abs(block[i][0][0] - block[i][1][0]) < 1 or abs(block[i][0][1] - block[i][1][1]) < 1:
            point1.append([min(a[:, 0]), min(a[:, 1]), min(a[:, 2]), 2])
            point2.append([max(a[:, 0]), max(a[:, 1]), max(a[:, 2])])
        else:
            point_1[0] = a[0]
            u = 1
            high = max(a[:, 2]) - min(a[:, 2])
            for j in range(1, 8):
                if a[j][2] == min(a[:, 2]):
                    point_1[u] = a[j]
                    u += 1
            new_point = point_area(point_1, high)
            point1 = point1 + new_point[0]
            point2 = point2 + new_point[1]
    point1 = np.array(point1)
    point2 = np.array(point2)
    a1 = shape(pip)[0]
    start = np.zeros((a1, 3))
    goal = np.zeros((a1, 3))
    diameter = np.zeros(a1)

    for i in range(a1):
        start[i] = pip[i][0]
        goal[i] = pip[i][1]
        diameter[i] = max(pip[i][2], pip[i][3])
    min1 = min(min(start[:, 0]), min(goal[:, 0]))
    min2 = min(min(start[:, 1]), min(goal[:, 1]))
    min3 = min(min(start[:, 2]), min(goal[:, 2]))
    max1 = max(max(start[:, 0]), max(goal[:, 0]), max(point1[:, 0]), max(point2[:, 0]))
    max2 = max(max(start[:, 1]), max(goal[:, 1]), max(point1[:, 1]), max(point2[:, 1]))
    max3 = max(max(start[:, 2]), max(goal[:, 2]), max(point1[:, 2]), max(point2[:, 2]))
    min_1 = [min1, min2, min3]
    area = [max1 - min1, max2 - min2, max3 - min3]
    for i in range(a1):
        for j in range(3):
            start[i][j] = int((start[i][j] - min_1[j]) / diameter[i])
            goal[i][j] = int((goal[i][j] - min_1[j]) / diameter[i])
    for i in range(a0):
        for j in range(3):
            point1[i][j] = point1[i][j] - min_1[j]
            if point1[i][j] < 0:
                point1[i][j] = 0
            point2[i][j] = point2[i][j] - min_1[j]
            if point2[i][j] < 0:
                point2[i][j] = 0

    start = np.array(start).astype(dtype=int).tolist()
    goal = np.array(goal).astype(dtype=int).tolist()
    num = "num1.png"
    print("起点终点", start, goal)
    # start[0][2]=start[0][2]+30
    # goal[0][2] = goal[0][2] + 30
    # print("起点终点", start, goal)
    print("point1, point2", point1, point2)
    a = run(point1, point2, start, goal, color, num, diameter, goal1, condition, min_1, pip, area)

    block_num1 = 0
    tem = pzjc(a, block)
    for i in range(len(tem)):
        if tem[i]:
            block_num1 += 1
    if block_num > block_num1:
        print("Astar算法生成的管道：", a)
        return a
    else:
        print("返回原管道：", pip)
        return pip


# pip2 = [[[68654.656351344, 142991.572411424, 54918.1], [73003.856351344, 142991.572411424, 54918.1], 600.0, 600.0],
#         [[73654.656351344, 142340.772411424, 54918.1], [73654.656351344, 137991.572411424, 54918.1], 600.0, 600.0]]
# block2 = [[[75009.824255584, 139908.640673971, 55150.0], [72489.824255584, 139920.640673972, 55150.0], 100.0, 4500.0]]
#
# goal1=[[],[]]
# color=[3,3]
# condition=[[50,0],[50,0]]
# #
# pip_block_to_point(block2,pip2,goal1,color,condition)
# # print(result_pip[0])
# pip_check_to_coordinate(result_pip[0],result_pip[1],250,min_num)


# print(result_pip)

# point1=data[0]
# point2=data[1]
# data=read_xml.read()

# a=read_xml.big_small(point1,point2)


# a=run1(point1,point2,(3, 0, 2),(3,10,2),3,"num1.png",300,1)
# b=pipeline_to_barrier1(point1,point2,a[0],a[1],a[2],300)
# c=run1(b[0],b[1],(12,0,3),(12,15,3),4,"num2.png",200,1)

blockList: [
    [[-15391.356861636, 8385.501937567, 18409.307488204], [-15391.356861821, 8385.506575033, 18322.0], 120.0, 120.0],
    [[-20289.148040414, 8385.514013261, 18013.786277545], [-15554.63124458, 8385.514013261, 18081.430271844], 170.0,
     170.0]]

# #场景2
# point1=[[0,1000,400,2]]
# point2=[[4000,3000,1500]]
# a=run1(point1,point2,(4, 5, 0),(10,13,10),"num1.png",300)

# #场景3 梁加墙壁
# point1=[[2000,0,600,2],[6000,0,600,2],[2600,2900,600,2],[2000,5400,600,2],[6000,5400,600,2],[2600,5400,600,2],[2400,3400,1500,2],[3900,3400,1500,2],[5400,3400,1500,2]]
# point2=[[2600,3400,1700],[6500,3400,1700],[6000,3400,1700],[2600,9000,1700],[6500,9000,1700],[6000,5800,1700],[2700,5400,1700],[4200,5400,1700],[5700,5400,1700]]
# color=[3,4,8,9,8,9,8,9]
# start=[(0,9,2),(0,8,2),(4,12,4),(4,17,4),(4,5,4),(4,25,4)]
# goal=[(15,13,2),(15,5,2),(23,5,4),(23,25,4),(4,11,4),(4,18,4)]
# num='dian_new.png'
# diameter=[500,500,300,300,300,300,300,300]
# run(point1,point2,start,goal,color,num,diameter)


# point1=[[0,0,2000,2],[0,0,6000,2],[0,2900,2600,2],[0,5400,2000,2],[0,5400,6000,2],[0,5400,2600,2],[1500,3400,2400,2],[1500,3400,3900,2],[1500,3400,5400,2]]
# point2=[[1700,3400,2600],[1700,3400,6500],[1700,3400,6000],[1700,9000,2600],[1700,9000,6500],[1700,5800,6000],[1700,5400,2700],[1700,5400,4200],[1700,5400,5700]]
# color=[3,4,8,9,8,9,8,9]
# start=[(2,9,0),(2,8,0),(4,12,4),(4,17,4),(4,5,4),(4,25,4)]
# goal=[(2,13,15),(2,5,15),(4,5,23),(4,25,23),(4,11,4),(4,18,4)]
# num='dian_new.png'
# diameter=[500,500,300,300,300,300,300,300]
# run(point1,point2,start,goal,color,num,diameter)


# point1=[[600,0,0,2],[5100,0,0,2],[700,1200,3000,2],[700,3000,3000,2],[700,4800,3000,2]]
# point2=[[700,6000,3500],[5300,6000,3500],[5100,1700,3500],[5100,3500,3500],[5100,5300,3500]]
# color=[3,4,5,6,4,4]
# start=[(2,0,2),(3,0,2),(7,0,4),(2,0,4)]
# goal=[(2,6,2),(3,6,2),(7,10,4),(2,10,4)]
# num='dian_hengliang.png'
# diameter=[1000,1000,600,600,600,600,400]
# run(point1,point2,start,goal,color,num,diameter) 


# a=run1(point1,point2,(5, 0, 2),(5,10,2),3,"num1.png",300,0)
# b=pipeline_to_barrier(point1,point2,a[0],a[1],a[2],300)
# c=run1(b[0],b[1],(12,0,3),(12,15,3),4,"num2.png",200,1)
